#include <Arduino.h>
#include <ch32v_def.h>
#include "utility/twi.h"
#include "core_debug.h"

#define MAX_DELAY 0xFFFFFFFFU

#define SLAVE_MODE_TRANSMIT 0
#define SLAVE_MODE_RECEIVE 1
#define SLAVE_MODE_LISTEN 2

#define I2C_PRESC_MAX 16U
#define I2C_SCLDEL_MAX 16U
#define I2C_SDADEL_MAX 16U
#define I2C_SCLH_MAX 256U
#define I2C_SCLL_MAX 256U
#define SEC2NSEC 1000000000UL

#ifndef I2C_VALID_TIMING_NBR
#define I2C_VALID_TIMING_NBR 8U
#endif
#define I2C_ANALOG_FILTER_DELAY_MIN 50U /* ns */
#ifndef I2C_ANALOG_FILTER_DELAY_MAX
#define I2C_ANALOG_FILTER_DELAY_MAX 260U /* ns */
#endif
#ifndef I2C_USE_ANALOG_FILTER
#define I2C_USE_ANALOG_FILTER 1U
#endif
#ifndef I2C_DIGITAL_FILTER_COEF
#define I2C_DIGITAL_FILTER_COEF 0U
#endif

typedef enum
{
    I2C_SPEED_FREQ_STANDARD,  /* 100 kHz */
    I2C_SPEED_FREQ_FAST,      /* 400 kHz */
    I2C_SPEED_FREQ_FAST_PLUS, /* 1 MHz */
    I2C_SPEED_FREQ_NUMBER     /* Must be the last entry */
} I2C_speed_freq_t;

typedef struct
{
    uint32_t input_clock; /* I2C Input clock */
    uint32_t timing;      /* I2C timing corresponding to Input clock */
} I2C_timing_t;

static I2C_timing_t I2C_ClockTiming[I2C_SPEED_FREQ_NUMBER] = {0};

typedef struct
{
    uint32_t freq;      /* Frequency in Hz */
    uint32_t freq_min;  /* Minimum frequency in Hz */
    uint32_t freq_max;  /* Maximum frequency in Hz */
    uint16_t hddat_min; /* Minimum data hold time in ns */
    uint16_t vddat_max; /* Maximum data valid time in ns */
    uint16_t sudat_min; /* Minimum data setup time in ns */
    uint16_t lscl_min;  /* Minimum low period of the SCL clock in ns */
    uint16_t hscl_min;  /* Minimum high period of SCL clock in ns */
    uint16_t trise;     /* Rise time in ns */
    uint16_t tfall;     /* Fall time in ns */
    uint8_t dnf;        /* Digital noise filter coefficient */
} I2C_Charac_t;

static const I2C_Charac_t I2C_Charac[] = {
    [I2C_SPEED_FREQ_STANDARD] =
        {
            .freq = 100000,
            .freq_min = 80000,
            .freq_max = 120000,
            .hddat_min = 0,
            .vddat_max = 3450,
            .sudat_min = 250,
            .lscl_min = 4700,
            .hscl_min = 4000,
            .trise = 640,
            .tfall = 20,
            .dnf = I2C_DIGITAL_FILTER_COEF,
        },
    [I2C_SPEED_FREQ_FAST] =
        {
            .freq = 400000,
            .freq_min = 320000,
            .freq_max = 480000,
            .hddat_min = 0,
            .vddat_max = 900,
            .sudat_min = 100,
            .lscl_min = 1300,
            .hscl_min = 600,
            .trise = 250,
            .tfall = 100,
            .dnf = I2C_DIGITAL_FILTER_COEF,
        },
    [I2C_SPEED_FREQ_FAST_PLUS] =
        {
            .freq = 1000000,
            .freq_min = 800000,
            .freq_max = 1200000,
            .hddat_min = 0,
            .vddat_max = 450,
            .sudat_min = 50,
            .lscl_min = 500,
            .hscl_min = 260,
            .trise = 60,
            .tfall = 100,
            .dnf = I2C_DIGITAL_FILTER_COEF,
        }};

static uint32_t i2c_computeTiming(uint32_t clkSrcFreq, uint32_t i2c_speed)
{
    uint32_t ret = 0xFFFFFFFFU;
    uint32_t valid_timing_nbr = 0;
    uint32_t ti2cclk;
    uint32_t ti2cspeed;
    uint32_t prev_error;
    uint32_t dnf_delay;
    uint32_t clk_min, clk_max;
    uint16_t scll, sclh;
    uint8_t prev_presc = I2C_PRESC_MAX;

    int32_t tsdadel_min, tsdadel_max;
    int32_t tscldel_min;
    uint8_t presc, scldel, sdadel;
    uint32_t tafdel_min, tafdel_max;

    if (i2c_speed < I2C_SPEED_FREQ_NUMBER)
    {
        if ((I2C_ClockTiming[i2c_speed].input_clock == clkSrcFreq) && (I2C_ClockTiming[i2c_speed].timing != 0U))
        {
            ret = I2C_ClockTiming[i2c_speed].timing;
        }
        else
        {
            I2C_ClockTiming[i2c_speed].input_clock = clkSrcFreq;

            ti2cclk = (SEC2NSEC + (clkSrcFreq / 2U)) / clkSrcFreq;
            ti2cspeed = (SEC2NSEC + (I2C_Charac[i2c_speed].freq / 2U)) / I2C_Charac[i2c_speed].freq;

            tafdel_min = (I2C_USE_ANALOG_FILTER == 1U) ? I2C_ANALOG_FILTER_DELAY_MIN : 0U;
            tafdel_max = (I2C_USE_ANALOG_FILTER == 1U) ? I2C_ANALOG_FILTER_DELAY_MAX : 0U;
        }

        /*
         * tDNF = DNF x tI2CCLK
         * tPRESC = (PRESC+1) x tI2CCLK
         * SDADEL >= {tf +tHD;DAT(min) - tAF(min) - tDNF - [3 x tI2CCLK]} / {tPRESC}
         * SDADEL <= {tVD;DAT(max) - tr - tAF(max) - tDNF- [4 x tI2CCLK]} / {tPRESC}
         */
        tsdadel_min = (int32_t)I2C_Charac[i2c_speed].tfall +
                      (int32_t)I2C_Charac[i2c_speed].hddat_min -
                      (int32_t)tafdel_min - (int32_t)(((int32_t)I2C_Charac[i2c_speed].dnf + 3) * (int32_t)ti2cclk);
        tsdadel_max = (int32_t)I2C_Charac[i2c_speed].vddat_max -
                      (int32_t)I2C_Charac[i2c_speed].trise -
                      (int32_t)tafdel_max - (int32_t)(((int32_t)I2C_Charac[i2c_speed].dnf + 4) * (int32_t)ti2cclk);
        /* {[tr+ tSU;DAT(min)] / [tPRESC]} - 1 <= SCLDEL */
        tscldel_min = (int32_t)I2C_Charac[i2c_speed].trise +
                      (int32_t)I2C_Charac[i2c_speed].sudat_min;
        if (tsdadel_min <= 0)
        {
            tsdadel_min = 0;
        }
        if (tsdadel_max <= 0)
        {
            tsdadel_max = 0;
        }

        /* tDNF = DNF x tI2CCLK */
        dnf_delay = I2C_Charac[i2c_speed].dnf * ti2cclk;

        clk_max = SEC2NSEC / I2C_Charac[i2c_speed].freq_min;
        clk_min = SEC2NSEC / I2C_Charac[i2c_speed].freq_max;

        prev_error = ti2cspeed;

        for (presc = 0; presc < I2C_PRESC_MAX; presc++)
        {
            for (scldel = 0; scldel < I2C_SCLDEL_MAX; scldel++)
            {
                /* TSCLDEL = (SCLDEL+1) * (PRESC+1) * TI2CCLK */
                uint32_t tscldel = (scldel + 1U) * (presc + 1U) * ti2cclk;
                if (tscldel >= (uint32_t)tscldel_min)
                {

                    for (sdadel = 0; sdadel < I2C_SDADEL_MAX; sdadel++)
                    {
                        /* TSDADEL = SDADEL * (PRESC+1) * TI2CCLK */
                        uint32_t tsdadel = (sdadel * (presc + 1U)) * ti2cclk;
                        if ((tsdadel >= (uint32_t)tsdadel_min) && (tsdadel <=
                                                                   (uint32_t)tsdadel_max))
                        {
                            if (presc != prev_presc)
                            {
                                valid_timing_nbr++;
                                if (valid_timing_nbr >= I2C_VALID_TIMING_NBR)
                                {
                                    return ret;
                                }
                                /* tPRESC = (PRESC+1) x tI2CCLK*/
                                uint32_t tpresc = (presc + 1U) * ti2cclk;
                                for (scll = 0; scll < I2C_SCLL_MAX; scll++)
                                {
                                    /* tLOW(min) <= tAF(min) + tDNF + 2 x tI2CCLK + [(SCLL+1) x tPRESC ] */
                                    uint32_t tscl_l = tafdel_min + dnf_delay + (2U * ti2cclk) + ((scll + 1U) * tpresc);
                                    /* The I2CCLK period tI2CCLK must respect the following conditions:
                                    tI2CCLK < (tLOW - tfilters) / 4 and tI2CCLK < tHIGH */
                                    if ((tscl_l > I2C_Charac[i2c_speed].lscl_min) &&
                                        (ti2cclk < ((tscl_l - tafdel_min - dnf_delay) / 4U)))
                                    {
                                        for (sclh = 0; sclh < I2C_SCLH_MAX; sclh++)
                                        {
                                            /* tHIGH(min) <= tAF(min) + tDNF + 2 x tI2CCLK + [(SCLH+1) x tPRESC] */
                                            uint32_t tscl_h = tafdel_min + dnf_delay + (2U * ti2cclk) + ((sclh + 1U) * tpresc);
                                            /* tSCL = tf + tLOW + tr + tHIGH */
                                            uint32_t tscl = tscl_l + tscl_h + I2C_Charac[i2c_speed].trise +
                                                            I2C_Charac[i2c_speed].tfall;
                                            if ((tscl >= clk_min) && (tscl <= clk_max) &&
                                                (tscl_h >= I2C_Charac[i2c_speed].hscl_min) && (ti2cclk < tscl_h))
                                            {
                                                int32_t error = (int32_t)tscl - (int32_t)ti2cspeed;
                                                if (error < 0)
                                                {
                                                    error = -error;
                                                }
                                                /* look for the timings with the lowest clock error */
                                                if ((uint32_t)error < prev_error)
                                                {
                                                    prev_error = (uint32_t)error;
                                                    ret = ((presc & 0x0FU) << 28) |
                                                          ((scldel & 0x0FU) << 20) |
                                                          ((sdadel & 0x0FU) << 16) |
                                                          ((sclh & 0xFFU) << 8) |
                                                          ((scll & 0xFFU) << 0);
                                                    prev_presc = presc;
                                                    /* Save I2C Timing found for further reuse (and avoid to compute again) */
                                                    I2C_ClockTiming[i2c_speed].timing = ret;
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }
    return ret;
}

static uint32_t i2c_getTiming(i2c_t *obj, uint32_t frequency)
{
    uint32_t ret = 0;
    uint32_t i2c_speed = 0;
    if (frequency <= 100000)
    {
        i2c_speed = 100000;
    }
    else if (frequency <= 400000)
    {
        i2c_speed = 400000;
    }
    else if (frequency <= 1000000)
    {
        i2c_speed = 1000000;
    }

    if (i2c_speed != 0U)
    {
        ret = i2c_computeTiming(i2c_getClkFreq(obj->i2c), I2C_SPEED_FREQ_STANDARD);
    }

    return ret;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
void i2c_init(i2c_t *obj)
{
    i2c_custom_init(obj, 100000, I2C_AcknowledgedAddress_7bit, 0x33);
}

void i2c_custom_init(i2c_t *obj, uint32_t timing, uint32_t addressingMode,
                     uint32_t ownAddress)
{
    if (obj != NULL)
    {
        I2C_TypeDef *i2c_sda = pinmap_peripheral(obj->sda, PinMap_I2C_SDA);
        I2C_TypeDef *i2c_scl = pinmap_peripheral(obj->sda, PinMap_I2C_SCL);

        if (i2c_sda == NP || i2c_scl == NP)
        {
            core_debug("ERROR: at least one I2C pin has no peripheral\n");
        }
        else
        {
            obj->i2c = pinmap_merge_peripheral(i2c_sda, i2c_scl);

            if (obj->i2c == NP)
            {
                core_debug("ERROR: I2C pins mismatch\n");
            }
            else
            {
#ifdef I2C1_BASE
                if (obj->i2c == I2C1)
                {
                    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
                    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
                    obj->irq = I2C1_EV_IRQn;
                    obj->irqER = I2C1_ER_IRQn;
                }
#endif
#ifdef I2C2_BASE
                if (obj->i2c == I2C2)
                {
                    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
                    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
                    obj->irq = I2C2_EV_IRQn;
                    obj->irqER = I2C2_ER_IRQn;
                }
#endif
                /* Configure I2C GPIO pins */
                pinmap_pinout(obj->scl, PinMap_I2C_SCL);
                pinmap_pinout(obj->sda, PinMap_I2C_SDA);

                NVIC_InitTypeDef NVIC_InitStructure;

                obj->typeDef.I2C_ClockSpeed = i2c_getTiming(obj, timing);
                obj->typeDef.I2C_Mode = I2C_Mode_I2C;
                obj->typeDef.I2C_DutyCycle = I2C_DutyCycle_16_9;
                obj->typeDef.I2C_OwnAddress1 = ownAddress;
                obj->typeDef.I2C_Ack = I2C_Ack_Enable;
                obj->typeDef.I2C_AcknowledgedAddress = addressingMode;

                I2C_Init(obj->i2c, &obj->typeDef);

                NVIC_InitStructure.NVIC_IRQChannel = obj->irq;
                NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
                NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
                NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
                NVIC_Init(&NVIC_InitStructure);

                I2C_ITConfig(obj->i2c, I2C_IT_EVT | I2C_IT_BUF, ENABLE);

                I2C_Cmd(obj->i2c, ENABLE);

                obj->slaveRxNbData = 0;
                obj->slaveMode = SLAVE_MODE_LISTEN;
            }
        }
    }
}

void i2c_deinit(i2c_t *obj)
{
    NVIC_DisableIRQ(obj->irq);
    I2C_DeInit(obj->i2c);
}

void i2c_setTiming(i2c_t *obj, uint32_t frequency)
{
    uint32_t f = i2c_getTiming(obj, frequency);
    I2C_Cmd(obj->i2c, DISABLE);
    obj->typeDef.I2C_ClockSpeed = f;
    I2C_Init(obj->i2c, &obj->typeDef);
    I2C_Cmd(obj->i2c, ENABLE);
}

i2c_status_e i2c_master_write(i2c_t *obj, uint8_t dev_address, uint8_t *data, uint16_t size)
{
    i2c_status_e ret = I2C_OK;
    uint32_t tickstart = getCurrentMillis();
    uint32_t delta = 0;
    uint32_t err = 0;
    uint8_t i = 0;

    if (size == 0)
    {
        ret = i2c_IsDeviceReady(obj, dev_address, 1);
    }
    else
    {
        I2C_AcknowledgeConfig(obj->i2c, ENABLE);
        I2C_GenerateSTART(obj->i2c, ENABLE);
        while (!I2C_CheckEvent(obj->i2c, I2C_EVENT_MASTER_MODE_SELECT))
            ;

        I2C_Send7bitAddress(obj->i2c, 0x02, I2C_Direction_Transmitter);
        while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
            ;

        while (i < size)
        {
            if (i < size - 1)
            {
                if (I2C_GetFlagStatus(obj->i2c, I2C_FLAG_TXE) != RESET)
                {
                    I2C_SendData(obj->i2c, data[i]);
                    i++;
                }
            }
        }
        while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED))
            ;
        I2C_GenerateSTOP(obj->i2c, ENABLE);
    }
    return ret;
}

i2c_status_e i2c_slave_write_IT(i2c_t *obj, uint8_t *data, uint16_t size)
{
    uint8_t i = 0;
    i2c_status_e ret = I2C_OK;

    if (size > I2C_TXRX_BUFFER_SIZE)
    {
        ret = I2C_DATA_TOO_LONG;
    }
    else
    {
        for (i = 0; i < size; i++)
        {
            obj->i2cTxRxBuffer[obj->i2cTxRxBufferSize + i] = *(data + i);
        }

        obj->i2cTxRxBufferSize += size;
    }
    return ret;
}

i2c_status_e i2c_IsDeviceReady(i2c_t *obj, uint8_t devAddr, uint32_t trials)
{
    i2c_status_e ret = I2C_OK;

    uint32_t mstart = millis();

    FlagStatus status = I2C_GetFlagStatus(obj->i2c, I2C_FLAG_BUSY);
    if (status == SET)
    {
        return I2C_BUSY;
    }
    return I2C_OK;
}

i2c_t *get_i2c_obj(I2C_TypeDef *i2cx)
{
    i2c_t *obj = i2cx - offsetof(struct i2c_s, i2c);
    return (obj);
}

void i2c_attachSlaveRxEvent(i2c_t *obj, void (*function)(i2c_t *))
{
    if ((obj != NULL) && (function != NULL))
    {
        obj->i2c_onSlaveReceive = function;
    }
}

void i2c_attachSlaveTxEvent(i2c_t *obj, void (*function)(i2c_t *))
{
    if ((obj != NULL) && (function != NULL))
    {
        obj->i2c_onSlaveTransmit = function;
    }
}

void I2C_EV_IRQHandler(I2C_TypeDef *I2Cx)
{
    i2c_t *obj = get_i2c_obj(I2Cx);

    switch (I2C_GetLastEvent(I2Cx))
    {
    case I2C_EVENT_SLAVE_BYTE_TRANSMITTED:
    {
        I2C_SendData(I2Cx, obj->i2cTxRxBuffer[obj->slaveTxNbData]);
        obj->slaveTxNbData = 0;
        break;
    }
    case I2C_EVENT_SLAVE_BYTE_TRANSMITTING:
    {
        I2C_SendData(I2Cx, obj->i2cTxRxBuffer[obj->slaveTxNbData++]);
        break;
    }
    case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED:
    {
        if (obj->slaveMode == SLAVE_MODE_RECEIVE)
        {
            obj->i2c_onSlaveReceive(obj);
            obj->slaveMode = SLAVE_MODE_LISTEN;
            obj->slaveRxNbData = 0;
        }
        break;
    }
    case I2C_EVENT_SLAVE_BYTE_RECEIVED:
    {
        if (obj->slaveRxNbData < I2C_TXRX_BUFFER_SIZE)
        {
            obj->slaveMode = SLAVE_MODE_RECEIVE;
            obj->i2cTxRxBuffer[obj->slaveRxNbData++] = I2C_ReceiveData(I2Cx);
        }
        else
        {
            core_debug("ERROR: I2C Slave RX overflow\n");
        }

        break;
    }
    case I2C_EVENT_SLAVE_STOP_DETECTED:
    {
        I2C_Cmd(I2Cx, ENABLE);
        obj->slaveRxNbData = 0;
        obj->slaveMode = SLAVE_MODE_LISTEN;
        break;
    }
    default:
        break;
    }
}

void __func_interrupt I2C1_EV_IRQHandler(void)
{
    I2C_EV_IRQHandler(I2C1);
}

void __func_interrupt I2C2_EV_IRQHandler(void)
{
    I2C_EV_IRQHandler(I2C2);
}